#pragma once

#include <vector>
#include <algorithm>
#include <fstream>
#include <unordered_map>
#include <ceres/ceres.h>
#include "ceres/DataManager.h"
#include "../DataStructure/Frame.h"

#include "ceres/PBABlocks.hpp"

#include "Calibration.h"


// #include "PoseTransBlock.h"

namespace Estimator{

class initialization
{
  public:
    initialization() = delete;
    virtual ~initialization() = default;

    initialization(const Calibration &calibration) : mCalibration_(calibration) {
        SetOptions();
        // mvPoseTransBlock.reserve(100000);
    }

    void InitializeByManager(const Managing *pManager);
    void GenerateCostFunctionBeta(const Managing *pManaging);
    // void GenerateCostFunction(const Managing *pManaging);
    void AddPoseBlock(const Frame* pFrame);

    void SetOptions()
    {
      mOptions_.minimizer_type = ceres::TRUST_REGION;
      //mOptions_.linear_solver_type = ceres::DENSE_SCHUR;
      mOptions_.linear_solver_type = ceres::DENSE_SCHUR;
      mOptions_.trust_region_strategy_type = ceres::DOGLEG;
      mOptions_.minimizer_progress_to_stdout = true;
      mOptions_.max_num_iterations = 16;
      mOptions_.dogleg_type = ceres::SUBSPACE_DOGLEG;
      mOptions_.use_nonmonotonic_steps = true;
      mOptions_.num_threads = 4;
      mOptions_.num_linear_solver_threads = 1;
    }

    void SolveOptimization()
    {
        Eigen::Vector3d temp = Eigen::Vector3d::Zero();
        mHashPoseTransBlock[4].setEstimate(temp); 
        ceres::Solver::Summary summary;
        ceres::Solve(mOptions_, &mOptimizationProblem, &summary);
        std::cout << summary.FullReport() << "\n";
    }

    std::vector< std::pair<size_t ,std::array<double,3>> > mvPoseGroundTruth;

    void PrintPoseGT()
    {
        // mvPoseGroundTruth.reserve(mvPoseTransBlock.size());
        // for(auto ite = mvPoseTransBlock.cbegin(); ite != mvPoseTransBlock.cend(); ite++)
        // {
        //     std::array<double,3> temppose;
        //     std::copy(ite->parameters(), ite->parameters() + 3, temppose.begin());
        //     mvPoseGroundTruth.push_back(std::make_pair(ite->GetId(),temppose));
        // }
    }

    void WriteResultintoFile(std::string resultfile)
    {
        this->PrintPoseGT();
        std::fstream file(resultfile + "linearposeValue.txt", std::fstream::out | std::fstream::trunc);
        for (size_t i = 0; i < mvPoseGroundTruth.size(); ++i)
        {
            file << mvPoseGroundTruth[i].first << " ";
            for (size_t j = 0; j < 3; ++j)
            {
                file << mvPoseGroundTruth[i].second.at(j) << " ";
            }
            file << std::endl;
        }
        file.close();
        // landmarkfile.close();
    }

    void UpdateManager( Managing *pManager)
    {
        for( auto ite = pManager->mvpFrameContainer_.begin(); ite != pManager->mvpFrameContainer_.end(); ite++)
        {
            size_t index = (*ite)->GetFrameId();
            (*ite)->mTranslationVec = mHashPoseTransBlock.at(index).getEstimate();
        }
    }

  private:
    const Calibration mCalibration_;

    ceres::Problem mOptimizationProblem;   // The linearlization problem, 
    ceres::Solver::Options mOptions_;

    // Hash map for pose translation block. The index is the frame id. We can asscess
    // the corresponding block by the frame id.
    std::unordered_map<size_t/*frame id*/,PoseTransBlock> mHashPoseTransBlock;
};
}